home *** CD-ROM | disk | FTP | other *** search
/ C/C++ Users Group Library 1996 July / C-C++ Users Group Library July 1996.iso / listings / v_13_06 / williams / matapp.c < prev    next >
Encoding:
Text File  |  1995-03-27  |  1.8 KB  |  61 lines
  1. /* ------------------------- */
  2. /* Calculate filter gain (H) */
  3. /* ------------------------- */
  4. matmpy(S, j, j, M, n, j, Q, ABT, P);
  5.  
  6. matmpy(M, n, j, Q, j, n, R, AB, CPP);
  7. unused = augvrt(R, n, n, n);
  8. matmpy(Q, j, n, R, n, n, H, AB, P);
  9.  
  10. /* -------------------------------------- */
  11. /* Update S - Error estimation covariance */
  12. /* -------------------------------------- */
  13. /* ------------------------------ */
  14. /* First form (I - HM)S = S - HMS */
  15. /* Calculate Q = -HM           */
  16. /* ------------------------------ */
  17. matmpy(H, j, n, M, n, j, Q, AB, MP);
  18.  
  19. /* ------------------------------ */
  20. /* Now, form W = -HMS          */
  21. /* ------------------------------ */
  22. matmpy(Q, j, j, S, j, j, W, AB, P);
  23.  
  24. /* ------------------------------ */
  25. /* Call the most-excellent madsub */
  26. /* function to get S - HMS --> S  */
  27. /* AB  - treat S and W normally   */
  28. /*       (no transposes involved) */
  29. /* ADD - causes madsub to add     */
  30. /* ------------------------------ */
  31. madsub(S, j, j, W, j, j, S, j, j, AB, ADD);
  32.  
  33. /* ------------------------------ */
  34. /* Now, W = SQ' = (S - HMS)(-HM)' */
  35. /* ------------------------------ */
  36. matmpy(S, j, j, Q, j, j, W, ABT, P);
  37.  
  38. /* ------------------------------ */
  39. /* Finish augend          */
  40. /*  S = S + W              */
  41. /*    = (S - HMS)+(S - HMS)(-HM') */
  42. /*    = (S - HMS)-(S - HMS)HM'    */
  43. /*    = (S - HMS)(I - HM')      */
  44. /*    = (I - HM)S(I - HM)'      */
  45. /* As required                    */
  46. /* ------------------------------ */
  47. madsub(S, j, j, W, j, j, S, j, j, AB, ADD);
  48.  
  49. /* ---------------------------- */
  50. /* Finish update of S           */
  51. /* First, calculate addend HRH' */
  52. /* ---------------------------- */
  53. matmpy(H, j, n, R, n, n, Q, AB, P);
  54. matmpy(Q, j, n, H, j, n, W, ABT, P);
  55.  
  56. /* ---------------------------- */
  57. /* Finally, S + W -> S += HRH'    */
  58. /* ---------------------------- */
  59. madsub(S, j, j, W, j, j, S, j, j, AB, ADD);
  60.  
  61.